"jikkenr160_4_8_16" VRSJ AC 2020

Plot

Masahiro Furukawa

Sept 2, 2020

Opti Track Coordination from Hirayama

image.png

Coordination : Right Hand

Robot : +X(to Front), +Y(to Left), +Z(to Top)

Opti : +Z(to Front), +X(to Left), +Y(to Top)

In [43]:
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

df =  pd.read_csv("./jikkenr160_0902.csv", header = 0, skiprows = 6)
sw =  pd.read_csv("./r160_f80_f02_P81newsweep.csv",names=('x', 'y'))

df['X(mm)'] = df['X']*1000.0
df['Y(mm)'] = df['Y']*1000.0
df['Z(mm)'] = df['Z']*1000.0

fps_sweep = 60
n=len(sw['x'])
t=np.linspace(0,n,n)/fps_sweep
plt.figure(figsize=(20,3))
plt.plot(t,sw['x'])
plt.title('Sweep Signal')
plt.xlabel('t [mm]')
plt.ylabel('x, y [mm]')

plt.figure(figsize=(20,3))
f,t_,Sxx = signal.spectrogram(sw['x'], fps_sweep, nperseg=512)
ax = plt.gca()
ax.pcolormesh(t_, f, Sxx,vmax=1e+1, vmin=1e-17, shading='gouraud', cmap='gray') #coolwarm jet gray 
ax.set_ylim([0,fps_sweep/4])
plt.title('Sweep Signal')
ax.set_xlabel(u'Time [sec]')
ax.set_ylabel(u'Frequency [Hz]')

plt.show()
In [44]:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d

def setname(i):
    # implemented from 
    # https://github.com/maeda-lab/Scaledown/blob/master/simulation/simulation_realtime_Ji_sending/Source.cpp

    # Order of names created... 1/4xy,yz,zx 1/8xy,yz,zx  1/16xy,yz,zx
    # by using "if" ,"%",and "/"
    
    name = 'scale_'

    # C++ source might have bug
    # the following arrangement should be correct
    # Masahiro Furukawa
    if i % 3 == 1:
        space = "xy" 
        movespace = 0
    elif i % 3 == 0:
        space = "yz"
        movespace = 1
    else:
        space = "zx"
        movespace = 2

    if int(i / 3) == 0: 
        scalesize = "1_4"
        scale = 0.25
        c = 'g'
    elif int(i / 3) == 1:
        scalesize = "1_8"
        scale = 0.125
        c = 'r'
    else:
        scalesize = "1_16"
        scale = 0.0625
        c = 'b'

    return (name + scalesize + space , scale, c)

def Coordination_Opti2Robot(x,y,z):
    
    #    Robot : +X(to Front), +Y(to Left), +Z(to Top)
    # **  Opti : +Z(to Front), +X(to Left), +Y(to Top)
    
    return (z, x, y)

def prep_axis(ax, str, ssx, ssy, ssn, key_x):
    ax.plot(ssx, ssy, ssn, 'k.', alpha=0.2, label= key_x, markersize=1)
    ax.plot(ssx, ssn, 'k.', zdir='y', zs= 80  ,alpha=0.1, markersize=1)
    ax.plot(ssy, ssn, 'k.', zdir='x', zs=-80  ,alpha=0.1, markersize=1)
    ax.plot(ssx, ssy, 'k.', zdir='z', zs=-80  ,alpha=0.1, markersize=1)        
    ax.set_xlabel('Arm Position X [mm]')
    ax.set_ylabel('Arm Position Y [mm]')
    ax.set_zlabel('Arm Position Z [mm]')
    ax.set_xlim([-80, 80])
    ax.set_ylim([-80, 80])
    ax.set_zlim([-80, 80])
    ax.legend([str])

def plot_One(key_x, key_y, key_z):   
    
    fps_opti = 120 
    n = len(df[key_x])
    t = np.linspace(0,n,n)/fps_opti
    print(n)
    
    deltaSec = 160
    plotSec = 10
    
    nTrial = int(n/deltaSec/fps_opti)
 
    alpha = 0.2
    
    fig = plt.figure(figsize=(20,15))
    fig.suptitle('Measured End-effector Trajectory in World Coordinate of Robot (no re-scaled)', fontsize=30)
        
    start_in_sec = [2, 167, 166*2+10, 166*3+4, 166*4, 166*5, 166*6, 166*7, 166*8, 166*9] # correct @ 120fps
    
    for s in range(nTrial):
        
        str,_,_ = setname(s)
        
        (x,y,z) = Coordination_Opti2Robot(df[key_x], df[key_y], df[key_z])
        
        plt.subplot(3,3,s+1)
        plt.plot(t, z,'b-',alpha=0.9,label = key_z, markersize=3)
        plt.plot(t, x,'r-',alpha=0.9,label = key_x, markersize=3)
        plt.plot(t, y,'g-',alpha=0.9,label = key_y, markersize=3)
        plt.title('Target Signal as Sequential ' + str)
        plt.xlabel('Time [s]')
        plt.ylabel('Arm Position X, Y, Z [mm]')
#         plt.ylim([0,10])
        ts = start_in_sec[s]
        te = start_in_sec[s+1]
        plt.xlim([ts, te])
        plt.legend()
        plt.grid(True)
    
    plt.show()

    
    fig = plt.figure(figsize=(20,15))
    fig.suptitle('Measured End-effector Trajectory in World Coordinate of Robot (no re-scaled)', fontsize=30)
    
    for s in range(nTrial):
        
        str, scale, c = setname(s)
        
        ss = start_in_sec[s]   *fps_opti
        es = start_in_sec[s+1] *fps_opti
        
        x = ( df[key_x][ss:es] - np.mean(df[key_x][ss:es]) )
        y = ( df[key_y][ss:es] - np.mean(df[key_y][ss:es]) ) 
        z = ( df[key_z][ss:es] - np.mean(df[key_z][ss:es]) )
        
        (x,y,z) = Coordination_Opti2Robot(x,y,z)
        
        ax = fig.add_subplot(331+s, projection='3d')
        
        ax.plot(x, y, z, c, alpha=0.7)
        
        ax.plot(x, z, 'k-', zdir='y', zs= 80  ,alpha=0.2, c=c)
        ax.plot(y, z, 'k-', zdir='x', zs=-80  ,alpha=0.2, c=c)
        ax.plot(x, y, 'k-', zdir='z', zs=-80  ,alpha=0.2, c=c)


        sx = sw['x'][:plotSec*fps_opti] * scale
        sy = sw['y'][:plotSec*fps_opti] * scale    
        sn = np.zeros(len(sx))

        if 'xy' in str:
            ssx = sx
            ssy = sy
            ssn = sn
            
        if 'yz' in str:
            ssx = sn
            ssy = sx
            ssn = sy

        if 'zx' in str:
            ssx = sx
            ssy = sn
            ssn = sy

        prep_axis(ax, str, ssx, ssy, ssn, key_x)

    fig = plt.figure(figsize=(20,15))
    fig.suptitle('Measured End-effector Trajectory in World Coordinate of Robot (RE-SCALED)', fontsize=30)
    
    for s in range(nTrial):
        
        str, scale, c = setname(s)
        
        ss = start_in_sec[s]   *fps_opti
        es = start_in_sec[s+1] *fps_opti
        
        x = ( df[key_x][ss:es] - np.mean(df[key_x][ss:es]) ) / scale / 4.0
        y = ( df[key_y][ss:es] - np.mean(df[key_y][ss:es]) ) / scale / 4.0
        z = ( df[key_z][ss:es] - np.mean(df[key_z][ss:es]) ) / scale / 4.0
        
        (x,y,z) = Coordination_Opti2Robot(x,y,z)
        
        ax = fig.add_subplot(331+s, projection='3d')
        
        ax.plot(x, y, z, c, alpha=0.7)
        
        ax.plot(x, z, 'k-', zdir='y', zs= 80  ,alpha=0.2, c=c)
        ax.plot(y, z, 'k-', zdir='x', zs=-80  ,alpha=0.2, c=c)
        ax.plot(x, y, 'k-', zdir='z', zs=-80  ,alpha=0.2, c=c)
        
        sx = sw['x'][:plotSec*fps_opti]  / 4.0
        sy = sw['y'][:plotSec*fps_opti]  / 4.0
        sn = np.zeros(len(sx))

        if 'xy' in str:
            ssx = sx
            ssy = sy
            ssn = sn
            
        if 'yz' in str:
            ssx = sn
            ssy = sx
            ssn = sy

        if 'zx' in str:
            ssx = sx
            ssy = sn
            ssn = sy
        
        prep_axis(ax, str, ssx, ssy, ssn, key_x)

    fig = plt.figure(figsize=(10,7))
    fig.suptitle('Measured End-effector Trajectory in World Coordinate of Robot (RE-SCALED)', fontsize=30)
    
    ax = fig.add_subplot(111, projection='3d')
    
    for s in [1,4,7]:
        
        str, scale, c = setname(s)
        
        ss = start_in_sec[s]   *fps_opti
        es = start_in_sec[s+1] *fps_opti
        
        x = ( df[key_x][ss:es] - np.mean(df[key_x][ss:es]) ) / scale / 4.0
        y = ( df[key_y][ss:es] - np.mean(df[key_y][ss:es]) ) / scale / 4.0
        z = ( df[key_z][ss:es] - np.mean(df[key_z][ss:es]) ) / scale / 4.0
        
        (x,y,z) = Coordination_Opti2Robot(x,y,z)
        
        ax.plot(x, y, z, c, alpha=0.6, label=str)

        ax.plot(x, z, 'k-', zdir='y', zs= 80  ,alpha=0.2, c=c)
        ax.plot(y, z, 'k-', zdir='x', zs=-80  ,alpha=0.2, c=c)
        ax.plot(x, y, 'k-', zdir='z', zs=-80  ,alpha=0.2, c=c)

        sx = sw['x'][:plotSec*fps_opti] / 4.0
        sy = sw['y'][:plotSec*fps_opti] / 4.0
        sn = np.zeros(len(sx))
        
        if 'xy' in str:
            ssx = sx
            ssy = sy
            ssn = sn
            
        if 'yz' in str:
            ssx = sn
            ssy = sx
            ssn = sy

        if 'zx' in str:
            ssx = sx
            ssy = sn
            ssn = sy       

        prep_axis(ax, str, ssx, ssy, ssn, key_x)

#     plt.tight_layout()
    plt.show()
    
plot_One('X(mm)', 'Y(mm)', 'Z(mm)')
178928
C:\Users\Masahiro Furukawa\.conda\envs\python3.7\lib\site-packages\IPython\core\pylabtools.py:132: UserWarning: Creating legend with loc="best" can be slow with large amounts of data.
  fig.canvas.print_figure(bytes_io, **kw)
C:\Users\Masahiro Furukawa\.conda\envs\python3.7\lib\site-packages\IPython\core\pylabtools.py:132: UserWarning: Creating legend with loc="best" can be slow with large amounts of data.
  fig.canvas.print_figure(bytes_io, **kw)
In [1]:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
from scipy.signal import find_peaks
from scipy.signal import correlate

# ===========-sweep曲線を作成する========
# Hirayama said;
# sweep信号については条件を変えました。(共有忘れていました)
# fmax:8.0Hzfmin0.2Hz、データ数は8196+冒頭分です。
# 冒頭25秒間は0.2Hzで運動させて、そこからsweep曲線が送られます。

f_max =       8   #  Hz
f_min =     0.2  #  Hz
dulation_s = 25   # s
n = 8196*2 # 8196 @ 60 fps = 8196 * 2 @ 120fps 

fps_opti =   120 # Hz
nbuf = 6000

r = 160 # [mm]

delta_f = (f_max - f_min) / n

ns = dulation_s * fps_opti
t1 = np.linspace(0, ns, ns) / fps_opti
t2 = np.linspace(0, n, n) / fps_opti + dulation_s
t = np.append(t1,t2)

# (単位円の半径r)*(PI)*(時刻tにおける振動数f)*(時刻t)
sx120 = -r * np.sin(2.0 * np.pi *  f_min * t[:dulation_s*fps_opti])
sx120s= -r * np.sin(2.0 * np.pi * (f_min + delta_f * np.linspace(0,n,n)) * np.linspace(0,n,n)/fps_opti)
sx120 = np.append(sx120, sx120s)
plt.figure(figsize=(20,2))
plt.plot(t[:nbuf], sx120[:nbuf])
plt.title('input signal')
plt.show()


def setname(i):
    # implemented from 
    # https://github.com/maeda-lab/Scaledown/blob/master/simulation/simulation_realtime_Ji_sending/Source.cpp

    # Order of names created... 1/4xy,yz,zx 1/8xy,yz,zx  1/16xy,yz,zx
    # by using "if" ,"%",and "/"
    
    name = 'scale_'

    # C++ source might have bug
    # the following arrangement should be correct
    # Masahiro Furukawa
    if i % 3 == 1:
        space = "xy" 
        movespace = 0
    elif i % 3 == 0:
        space = "yz"
        movespace = 1
    else:
        space = "zx"
        movespace = 2

    if int(i / 3) == 0: 
        scalesize = "1_4"
        scale = 0.25
        c = 'g'
    elif int(i / 3) == 1:
        scalesize = "1_8"
        scale = 0.125
        c = 'r'
    else:
        scalesize = "1_16"
        scale = 0.0625
        c = 'b'

    return (name + scalesize + space , scale, c)

def Coordination_Opti2Robot(x,y,z):
    
    #    Robot : +X(to Front), +Y(to Left), +Z(to Top)
    # **  Opti : +Z(to Front), +X(to Left), +Y(to Top)
    
    return (z, x, y)

def plot_One(key_x, key_y, key_z):   
    
    fps_opti = 120
    n = len(df['x(mm)'])
    t = np.linspace(0,n,n)/fps_opti
    
    deltaSec = 50
    plotSec = 10
    
    nTrial = int(n/deltaSec/fps_opti)
 
    alpha = 0.2
    
    fig = plt.figure(figsize=(20,15))
    fig.suptitle('Measured End-effector Trajectory in World Coordinate of Robot (no re-scaled)', fontsize=30)
        
    start_in_sec = [10, 110, 247, 375, 475, 575, 675, 775, 875, 975] # incorrect @ 60fps
    start_in_sec = [5,  55,  123, 188, 238, 288, 338, 388, 438, 488] # correct @ 120fps
    
    (x,y,z) = Coordination_Opti2Robot(df[key_x], df[key_y], df[key_z])

    for s in [1,4,7]:
        
        str,scale,_ = setname(s)
        
        ts = start_in_sec[s]
        te = ts + deltaSec
        
        xx = x[ts*fps_opti : te*fps_opti]
        xx = xx - np.mean(xx)
        n = len(xx)
        t = np.linspace(0,n,n)/fps_opti
        
        peaks_xx, _ = find_peaks(xx,    distance=40)
        peaks_sx, _ = find_peaks(sx120, distance=40)
#         acf = correlate(x,x)[acTSec*fs:]

        plt.subplot(3,1,int(s/3)+1)
        plt.plot(t, xx, 'r-', alpha=0.9, label = key_x, markersize=2)
        plt.plot(t+.5, sx120[:deltaSec*fps_opti]*scale,'k-', alpha=0.3,label = key_x+' SWEEP', markersize=3)
        plt.title('Target Signal as Sequential ' + str)
        plt.xlabel('Time [s]')
        plt.ylabel('Arm Position X, Y, Z [mm]')
        plt.ylim([-50,50])
        plt.legend()
        plt.grid(True)


#     plt.tight_layout()
    plt.show()
    
# plot_One('z(mm)', 'x(mm)', 'y(mm)')
plot_One('x(mm)', 'y(mm)', 'z(mm)')
---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-1-785563fe4411> in <module>
     23 
     24 ns = dulation_s * fps_opti
---> 25 t1 = np.linspace(0, ns, ns) / fps_opti
     26 t2 = np.linspace(0, n, n) / fps_opti + dulation_s
     27 t = np.append(t1,t2)

NameError: name 'np' is not defined
In [33]:
# Masahiro Furukawa
# Aug 31, 2020

# Spectrogram

## coding:utf-8
import os
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from scipy import signal
from scipy import fftpack

def plot_Spec(key_x, key_y, key_z):
    
    for s in [0,3,6]:

        fps_opti = 60
        deltaSec = 53
        

        str, scale, c = setname(s)

        ss = s*deltaSec*fps_opti
        es = (s+1)*deltaSec*fps_opti

        x = ( df[key_x][ss:es] - np.mean(df[key_x][ss:es]) ) / scale / 4.0
        y = ( df[key_y][ss:es] - np.mean(df[key_y][ss:es]) ) / scale / 4.0
        z = ( df[key_z][ss:es] - np.mean(df[key_z][ss:es]) ) / scale / 4.0
        
        (x,y,z) = Coordination_Opti2Robot(x,y,z)

        fig = plt.figure(figsize=(10,4))

        vmx = [1e-9, 5*1e-7]
        vmn = [1e-11, 1e-8]

        # spectrogram
        f,t_,Sxx = signal.spectrogram(x, fps_opti, nperseg=128)
        ax = plt.gca()
        ax.pcolormesh(t_, f, Sxx,vmax=1e+1, vmin=1e-17, shading='gouraud', cmap='gray') #coolwarm jet gray 
        # ax.set_xlim([150,400])
        ax.set_ylim([0,fps_opti/4])
        ax.set_title(str)
        ax.set_xlabel(u'Time [sec]')
        ax.set_ylabel(u'Frequency [Hz]')

plot_Spec('x(mm)', 'y(mm)', 'z(mm)')
In [253]:
print(np.power(2,12))
4096